#include "MPC.h"
#include <math.h>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>
#include <vector>
//#include "Eigen-3.3/Eigen/Core"
#include <Eigen/Core>

using CppAD::AD;
using Eigen::VectorXd;


size_t N = 25; // 预测步长
double dt = 0.05; // 采样时间间隔
const double Lf = 2.67; // 前轮到质心得距离
double ref_v = 40; // 参考速度

// 解算器将所有状态变量和执行器变量放在一个单一的向量中。
// 此处定义了每个变量在向量中的启示位置
size_t x_start = 0;
size_t y_start = x_start + N;
size_t psi_start = y_start + N;
size_t v_start = psi_start + N;
size_t cte_start = v_start + N;
size_t epsi_start = cte_start + N;
size_t delta_start = epsi_start + N;
size_t a_start = delta_start + N - 1;

class FG_eval 
{
 public:
  VectorXd coeffs;

  // 参考路径的方程系数
  FG_eval(VectorXd coeffs) { this->coeffs = coeffs; }

  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;

  // 计算成本函数
  // ‘fg’ 是一个包含成本和车辆模型约束的向量。
  // ‘vars’ 是一个包含变量值（状态和执行器）的向量。
  void operator()(ADvector& fg, const ADvector& vars)
  {
    fg[0] = 0; // 成本存储在fg的第一个元素中

    // 基于参考路径成本部分。
    for (int t = 0; t < N; ++t) {
      fg[0] += CppAD::pow(vars[cte_start + t], 2);      // 横向误差
      fg[0] += CppAD::pow(vars[epsi_start + t], 2);     // 航行角误差
      fg[0] += CppAD::pow(vars[v_start + t] - ref_v, 2);// 速度误差
    }

    // 减小执行器的变化率，以保证转向是舒缓平滑的
    for (int t = 0; t < N - 1; ++t) {
      fg[0] += CppAD::pow(vars[delta_start + t], 2);
      fg[0] += CppAD::pow(vars[a_start + t], 2);
    }

    // 减少连续行为之间的值差距
    for (int t = 0; t < N - 2; ++t) {
      fg[0] += CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
      fg[0] += CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
    }

    // 设置约束
    // 我们根据初始状态初始化模型。回想一下fg[0]是预留给成本值的，所以其他索引会向前移动 1。
    fg[1 + x_start] = vars[x_start];      // x 坐标
    fg[1 + y_start] = vars[y_start];      // y 坐标
    fg[1 + psi_start] = vars[psi_start];  // psi
    fg[1 + v_start] = vars[v_start];      // v_start
    fg[1 + cte_start] = vars[cte_start];  // cte
    fg[1 + epsi_start] = vars[epsi_start];// epsi

    // 约束的其他部分
    for (int t = 1; t < N; ++t) {
      // t+1时刻的状态值
      AD<double> x1 = vars[x_start + t];        // x_{t+1}
      AD<double> y1 = vars[y_start + t];        // y_{t+1}
      AD<double> psi1 = vars[psi_start + t];    // psi_{t+1}
      AD<double> v1 = vars[v_start + t];        // v_{t+1}
      AD<double> cte1 = vars[cte_start + t];    // ctel_{t+1}
      AD<double> epsi1 = vars[epsi_start + t];  // epsil_{t+1}

      // t时刻的状态值
      AD<double> x0 = vars[x_start + t - 1];      // x0_t
      AD<double> y0 = vars[y_start + t - 1];      // y0_t
      AD<double> psi0 = vars[psi_start + t - 1];  // psi0_{t}
      AD<double> v0 = vars[v_start + t - 1];      // v0_{t}
      AD<double> cte0 = vars[cte_start + t - 1];  // cte0_{t}
      AD<double> epsi0 = vars[epsi_start + t - 1];// epsi0_{t}

      // 只考虑时间 t 的执行动作。
      AD<double> delta0 = vars[delta_start + t - 1];  // delta0
      AD<double> a0 = vars[a_start + t - 1];          // a0

      AD<double> f0 = coeffs[0] + coeffs[1] * x0; // 因为是直线
      AD<double> psides0 = CppAD::atan(coeffs[1]);

      // 这是 `x` 的起始值。
      // 这里的想法是将这个值限制为 0。
      //
      // 重新调用运动学模型的方程式
      // 用下一个状态的值，并将其与模型方程的预期值进行比较，设置为约束。
      // x_[t+1] = x[t] + v[t] * cos(psi[t]) * dt
      // y_[t+1] = y[t] + v[t] * sin(psi[t]) * dt
      // psi_[t+1] = psi[t] + v[t] / Lf * delta[t] * dt
      // v_[t+1] = v[t] + a[t] * dt
      // cte[t+1] = f(x[t]) - y[t] + v[t] * sin(epsi[t]) * dt
      // epsi[t+1] = psi[t] - psides[t] + v[t] * delta[t] / Lf * dt
      fg[1 + x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt); // 
      fg[1 + y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
      fg[1 + psi_start + t] = psi1 - (psi0 + v0 * delta0 / Lf * dt);
      fg[1 + v_start + t] = v1 - (v0 + a0 * dt);
      fg[1 + cte_start + t] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
      fg[1 + epsi_start + t] = epsi1 - ((psi0 - psides0) + v0 * delta0 / Lf * dt);
    }
  }
};


// MPC class definition
MPC::MPC() {}
MPC::~MPC() {}

// 给定初始状态求解模型，返回下一个状态和执行动作(以向量的形式返回)
// const VectorXd &x0: 车辆的状态向量(坐标, 航向角, 速度, 横向距离误差, 航向角度误差)
// const VectorXd &coeffs: 参考路径的方程系数
std::vector<double> MPC::Solve(const VectorXd &x0, const VectorXd &coeffs) 
{
  typedef CPPAD_TESTVECTOR(double) Dvector;

  // 提取车辆初始状态数值
  double x = x0[0];     // x坐标
  double y = x0[1];     // y坐标
  double psi = x0[2];   // 航向角
  double v = x0[3];     // 速度
  double cte = x0[4];   // 横向距离误差
  double epsi = x0[5];  // 航向角度误差

  // 定义存储变量值的长度: N组状态变量和(N-1)组控制变量
  // 因为状态变量包含初始状态，所以状态变量比控制变量多一组
  size_t n_vars = N * 6 + (N - 1) * 2;
  size_t n_constraints = N * 6; // 约束量的个数

  // 申请变量数组并进行初始化
  Dvector vars(n_vars);
  for (int i = 0; i < n_vars; ++i) {
    vars[i] = 0.0;
  }

  // 给变量数组中第一组状态变量赋初值(初值来自参数)
  vars[x_start] = x; // x坐标
  vars[y_start] = y; // y坐标
  vars[psi_start] = psi; // 航向角
  vars[v_start] = v; // 速度
  vars[cte_start] = cte; // 横向误差
  vars[epsi_start] = epsi; // 航行误差

  // 给变量值设置上限和下限
  Dvector vars_lowerbound(n_vars);
  Dvector vars_upperbound(n_vars);
  // 将所有状态变量的上下限设置为最大和最小负值
  for (int i = 0; i < delta_start; ++i) {
    vars_lowerbound[i] = -1.0e19;
    vars_upperbound[i] = 1.0e19;
  }

  // delta的上下限制被设置为-25和25度（以弧度为单位）。
  // 注意：可以随意更改这些值。
  for (int i = delta_start; i < a_start; ++i) {
    vars_lowerbound[i] = -0.436332;
    vars_upperbound[i] = 0.436332;
  }

  // 加速度/减速度的上下限制。
  // 注意：可以随意更改这些值。
  for (int i = a_start; i < n_vars; ++i) {
    vars_lowerbound[i] = -1.0;
    vars_upperbound[i] = 1.0;
  }

  // 约束的上下限制
  // 除了初始状态索引外，其余的都应该为0。
  Dvector constraints_lowerbound(n_constraints);
  Dvector constraints_upperbound(n_constraints);
  for (int i = 0; i < n_constraints; ++i) {
    constraints_lowerbound[i] = 0;
    constraints_upperbound[i] = 0;
  }
  // 初始状态变量的约束为初始值
  constraints_lowerbound[x_start] = x;
  constraints_lowerbound[y_start] = y;
  constraints_lowerbound[psi_start] = psi;
  constraints_lowerbound[v_start] = v;
  constraints_lowerbound[cte_start] = cte;
  constraints_lowerbound[epsi_start] = epsi;

  constraints_upperbound[x_start] = x;
  constraints_upperbound[y_start] = y;
  constraints_upperbound[psi_start] = psi;
  constraints_upperbound[v_start] = v;
  constraints_upperbound[cte_start] = cte;
  constraints_upperbound[epsi_start] = epsi;

  // 为求最优解作准备
  FG_eval fg_eval(coeffs);

  // options
  std::string options;
  options += "Integer print_level  0\n";
  options += "Sparse  true        forward\n";
  options += "Sparse  true        reverse\n";

  // 返回结果的地方
  CppAD::ipopt::solve_result<Dvector> solution;

  // 函数 ipopt::solve 解决的非线性规划问题形式如下：
  // minimize f(x)
  // subject to xl <= x <= xu; gl <= g(x) <= gu;
  CppAD::ipopt::solve<Dvector, FG_eval>(
                options, 
                vars, // 变量(包括状态变量和控制变量)数组
                vars_lowerbound, // 变量值的下限
                vars_upperbound, // 变量值的上限
                constraints_lowerbound, // 约束的下限
                constraints_upperbound, // 约束的上限
                fg_eval, // 
                solution); // 结果

  // 检查一些结果中的值。
  bool ok = true;
  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

  auto cost = solution.obj_value;
  std::cout << "Cost " << cost << std::endl;
  return {solution.x[x_start + 1],   solution.x[y_start + 1],
          solution.x[psi_start + 1], solution.x[v_start + 1],
          solution.x[cte_start + 1], solution.x[epsi_start + 1],
          solution.x[delta_start],   solution.x[a_start]};
}